import sensor, image, time
from pyb import UART
from pyb import LED


THRESHOLD = (0, 20)  # 黑色阈值
# OpenMV的灰度图: 0是纯黑, 255是纯白
COLOR = 0


SCREEN_W = 80
SCREEN_H = 60
ROI_W = 20
ROI_H = 50
ROI = (int(SCREEN_W/2 - ROI_W/2), int(SCREEN_H/2-ROI_H/2), ROI_W, ROI_H)

STOP_ROI_W = 5
STOP_ROI_H = 6

OFFSET = 15

STOP1_X = 20
STOP2_X = STOP1_X + OFFSET
STOP3_X = STOP2_X + OFFSET

STOP_Y = 0

STOP1_ROI = (STOP1_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)
STOP2_ROI = (STOP2_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)
STOP3_ROI = (STOP3_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)

uart = UART(3, 115200)

LED(1).on()
LED(2).on()
LED(3).on()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)

sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.set_pixformat(sensor.GRAYSCALE)
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames()     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.

while(True):
    clock.tick()

    img = sensor.snapshot()
    img = img.binary([THRESHOLD], invert=True)  # 阈值内的像素变为0(黑)

    roi_img = img.copy(roi=ROI, copy_to_fb=False)
    stop_img = img.copy(copy_to_fb=False)

    img.draw_rectangle(ROI, color = COLOR)

    # 判停框
    img.draw_rectangle(STOP1_ROI, color = 255)
    img.draw_rectangle(STOP2_ROI, color = 255)
    img.draw_rectangle(STOP3_ROI, color = 255)

    # 判停
    pix1 = stop_img.get_pixel(STOP1_ROI[0], STOP1_ROI[1] + 3)
    pix2 = stop_img.get_pixel(STOP2_ROI[0], STOP1_ROI[1] + 3)
    pix3 = stop_img.get_pixel(STOP3_ROI[0], STOP1_ROI[1] + 3)

    # print("pix1: %d, pix2: %d, pix3: %d" % (pix1, pix2, pix3))

    if pix1 == 0 and pix2 == 0 and pix3 == 0:
        # print("e")
        uart.write("e")
        continue


    #sta1 = stop_img.get_statistics([STOP1_ROI])
    #sta2 = stop_img.get_statistics([STOP2_ROI])
    #sta3 = stop_img.get_statistics([STOP3_ROI])
    #mode1 = sta1.mode()
    #mode2 = sta2.mode()
    #mode3 = sta3.mode()
    #print("mode1:%d, mode2:%d, mode3:%d" % (mode1, mode2, mode3))
    #if mode1 == 0 and mode2 == 0 and mode3 == 0:
        #print("e")
        #uart.write("e")
        ## break;
        #continue

    line = roi_img.get_regression([(0,0)], robust = True)
    if (line):

        # rho_err = abs(line.rho())-roi_img.width()/2

        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()

        # 画线性回归的线
        line_list = list(line.line())
        offset_x = int(SCREEN_W / 2 - ROI_W/2)
        offset_y = int(SCREEN_H / 2 - ROI_H/2)
        line_list[0] += offset_x;
        line_list[1] += offset_y;
        line_list[2] += offset_x;
        line_list[3] += offset_y;
        img.draw_line(tuple(line_list), color = COLOR)

        # print(rho_err,line.magnitude())

        if line.magnitude()>8:  # 线性回归效果好
            if theta_err > 20:  # 右转
                print("b")
                uart.write("b")
            elif theta_err > 10:
                print("a")
                uart.write("a")
            elif theta_err < -20:  # 左转
                print("d")
                uart.write("d");
            elif theta_err < -10:
                print("c")
                uart.write("c")
            else:
                print("f")
                uart.write("f")
        else:
            print("n")
            uart.write("n")
    else:
        print("x")
        uart.write("x")

    #print(clock.fps())
